/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
#include "PositionLogger.hpp"
#include "PositionFileWriter.hpp"
#include "gimutils.h"
// -*- coding:iso-8859-15 mode: c++ -*-
/** 
 * \file
 * \brief MaCI_Position Logger implementation.
 * \author Matthieu Myrsky <matthieu.myrsky@tkk.fi>
 *
 * $Id: PositionLogger.cpp,v 1.15 2009-11-16 16:07:38 morsko Exp $
 *
 */


PositionLogger::PositionLogger(MaCI::Position::CPositionClient *aClient,
                               std::string aMaCISLName):
  CSync(1, 1),               
  CThread(1),                
  Logger(),
  iPClient(aClient),
  iFileWriter(),
  iRunning(false),
  iRunningThreadStopped(true),
  iMaCISLName(aMaCISLName),
  iAdditionalInformation("Not active"),
  iMenu()
{

  iMenu[std::string("Timestamp")] = true;
  iMenu[std::string("Position")] = true;
  iMenu[std::string("Variances")] = true;
  iMenu[std::string("Probability")] = true;
  iMenu[std::string("Speed")] = true;
  /*

  iMenu = new wxMenu();
  // wxMenuItem *mItem = new wxMenuItem(NULL,0,"Log types");

  wxMenuItem *title = iMenu->Insert(0,0,"LOG TYPES - (timestamp must be checked for playing with MaCI_Player)");
 

  wxMenuItem *timestamp = iMenu->InsertCheckItem(1,1,"Timestamp");
  timestamp->Check(true);

  wxMenuItem *pos = iMenu->InsertCheckItem(2,2,"Position");
  pos->Check(true);


  wxMenuItem *var = iMenu->InsertCheckItem(3,3,"Variances");
  var->Check(true);

  wxMenuItem *prob = iMenu->InsertCheckItem(4,4,"Probability");
  prob->Check(true);

  wxMenuItem *spe = iMenu->InsertCheckItem(5,5,"Speed");
  spe->Check(true);
  */
}



PositionLogger::~PositionLogger(){
  if(iRunning){
    StopLogging();
  }
 
}

 
bool PositionLogger::StartLogging(void)
{
  dPrint(10,"Start called");
  
  if(!iRunning){

    /*  char buffer[256];
    dPrint(5,"Start logging position events");
    if(iFilenameSet){
  
      sprintf(buffer,"data/%s.txt",
             iFilename.c_str());
      
    }else{
    
      gim::time tstamp(true);
      tm t;
      //      MaCI::Common::TTimestamp tstamp;
      tstamp.getLocalTime(t);
       
      sprintf(buffer,"data/%s_%.2d_%.2d_%d_%.2d:%.2d:%.2d.txt",
              iMaCISLName.c_str(),
              t.tm_mday,
              t.tm_mon+1,
              t.tm_year+1900,
              t.tm_hour,
              t.tm_min,
              t.tm_sec);
              
     
    }

    std::string filename(buffer);

                     iFile = fopen(buffer,"wt");*/
    /*  for(int i = 0; i < iMenu->GetMenuItemCount();i++){
      iMenu->Enable(i,false);
      }*/
    iRunning = true; 
    RunThread(0);
    return true;
  }

  return false;
}

 
bool PositionLogger::StopLogging(void)
{

  dPrint(10,"stop called");
  if(iRunning){
    Lock();
    iRunning = false;

    Unlock();
    WaitThread(0);
    /*
    for(int i = 0; i < iMenu->GetMenuItemCount();i++){
      iMenu->Enable(i,true);
      }*/
    Lock();
    iAdditionalInformation = "Not active";   
    Unlock();
    iFileWriter.CloseFile();
    //fclose(iFile);
    return true;
  }
  return false;
}


int PositionLogger::ThreadFunction(const int aThreadNumber)
{

  using namespace MaCI::Position;
  dPrint(1,"thread %d", aThreadNumber);
  if(aThreadNumber == 0){

    //First the filename
    char buffer[256];
    dPrint(5,"Start logging position events");
    if(iFilenameSet){
  
      sprintf(buffer,"data/%s.txt",
             iFilename.c_str());
      
    }else{
    
      gim::time tstamp(true);
      tm t;
      //      MaCI::Common::TTimestamp tstamp;
      tstamp.getLocalTime(t);
       
      sprintf(buffer,"data/%s_%.2d_%.2d_%d_%.2d:%.2d:%.2d.txt",
              iMaCISLName.c_str(),
              t.tm_mday,
              t.tm_mon+1,
              t.tm_year+1900,
              t.tm_hour,
              t.tm_min,
              t.tm_sec);
              
     
    }

    std::string filename(buffer);



    dPrint(1,"thread running");
    CPositionData data;
    int seq = -1;
   
    //Check for the logging types
    bool logTimestamp = true;//iMenu->IsChecked(1);
    bool logPosition =  true;//iMenu->IsChecked(2);
    bool logVariances =  true;//iMenu->IsChecked(3);
    bool logProbability =  true;//iMenu->IsChecked(4);
    bool logSpeed =  true;//iMenu->IsChecked(5);
      /////////Search the group and instance names :)
    std::string instance;
    std::string group;

    MaCI::MaCICtrl::SMaCISL sl(iMaCISLName);
    if (sl.IsValidFQMaCISL()) {
      group = sl.GetDeviceGroup();
      instance = sl.GetInstance();
    }
    //////////
    iFileWriter.OpenFile(filename,
                         group,
                         instance);

    /*

    fprintf(iFile,"%%MaCI_Position log file\n");
    fprintf(iFile,"%%Group: %s Instance: %s\n",group.c_str(),instance.c_str());

    fprintf(iFile,"%%");
    if(logTimestamp){
      
      fprintf(iFile,"timestamp_sec timestamp_usec ");
    }
    if(logPosition){
     
      fprintf(iFile,"position_x, position_y position_angle ");
    }

    if(logVariances){
      
      fprintf(iFile,"variance_x, variance_y variance_angle ");
    }
    if(logProbability){
     
      fprintf(iFile,"probability ");
    }
    if(logSpeed){
      fprintf(iFile,"speed ");
    }

    fprintf(iFile,"\n\n");
    */
    while(iRunning){
      char buffer2[256];
      if(iPClient->GetPositionEvent(data,
                                    &seq, 
                                    2000)){
        iFileWriter.WriteLine(data);
        Lock();

        /*if(logTimestamp){
          const MaCI::Common::TTimestamp *tstamp = data.GetTimestamp();
          if(tstamp){
            fprintf(iFile,"%u %u ",tstamp->timestamp_s,tstamp->timestamp_us);
          }else{
            fprintf(iFile,"0 0 ");
          }
        }
        
        if(logPosition){
          const TPose2D *pos = data.GetPose2D();
          if(pos){
            fprintf(iFile,"%f %f %f ",pos->x,pos->y,pos->a);
          }else{
            fprintf(iFile,"0 0 0 ");
          }
        }

        if(logVariances){
          const TVariance2D *var = data.GetVariance2D();
          if(var){
            fprintf(iFile,"%f %f %f ",var->x,var->y,var->a);
          }else{
            fprintf(iFile,"0 0 0 ");
          }
        }

        if(logProbability){
          const TProbability *prob = data.GetProbability();
          if(prob){
            fprintf(iFile,"%f ",prob->probability);
          }else{
            fprintf(iFile,"0 ");
          }
        }
        
        if(logSpeed){
          const TSpeed2D *speed = data.GetSpeed2D();
          if(speed){
            fprintf(iFile,"%f %f ",speed->speed, speed->angularSpeed);
          }else{
            fprintf(iFile,"0 0 ");
          }
        }
        
        fprintf(iFile,"\n");
        fflush(iFile);
        */
        if(data.GetTimestamp() && data.GetPose2D()){
          sprintf(buffer2,"s: %u, us: %u, pos x: %.2f y: %.2f",
                  data.GetTimestamp()->timestamp_s,data.GetTimestamp()->timestamp_us,
                  data.GetPose2D()->x,data.GetPose2D()->y);
        
          iAdditionalInformation.clear();
          iAdditionalInformation.assign(buffer2);
        }
        Unlock();
      }
    }
    Lock();
    iRunningThreadStopped = true;
    Unlock();
    
  }

  return 0;

}

std::string PositionLogger::GetName(){
  return "Position Logger";
}

Logger::TMenu *PositionLogger::GetMenu(void){
  return &iMenu;
}

bool PositionLogger::SetMenuItem(std::string aItem, bool aValue){ 
  if(!iRunning){
    iMenu[aItem] =  aValue;
    return true;
  }else{
    return false;
  }
}

bool PositionLogger::GetAdditionalInformation(std::string &aInformation){
  Lock();
  aInformation = iAdditionalInformation;
  Unlock();
  return true;
}
